/* 
 * File:   SmoothFilter.cpp
 * Author: root
 * 
 * Created on October 27, 2015, 8:57 PM
 */

#include "SmoothFilter.h"

SmoothFilter* SmoothFilter::instance = 0;


SmoothFilter::SmoothFilter()
{
}

SmoothFilter* SmoothFilter::getInstance()
{
  if ( !SmoothFilter::instance )
  {
        SmoothFilter::instance = new SmoothFilter();
  }
  return SmoothFilter::instance;
}

SensorData* SmoothFilter::get()
{
    return &this->sensor;
}

void SmoothFilter::release()
{
    if ( this->getInstance() )
    {
        delete this->getInstance();
    }
}

//void SmoothFilter::update(const RawData *paraRaw)
//{
//    uint32 tempCnt = 0;
//
//    RawData tempRaw;
//
//    if (this->rawBuffer.cnt > 0)
//    {
//        if ((paraRaw->time <= this->rawBuffer.back()->time) 
//                || (paraRaw->time > (this->rawBuffer.back()->time + 100))) {
//            this->rawBuffer.bottom = 0;
//            this->rawBuffer.top = 0;
//            this->rawBuffer.cnt = 0;
//        }
//    }
//    
//    if (this->rawBuffer.cnt > 5) {
//        this->rawBuffer.bottom = (this->rawBuffer.bottom + 1) % 6;
//        this->rawBuffer.cnt--;
//    }
//    
//    this->rawBuffer.data[this->rawBuffer.top] = *paraRaw;
//    
//    ++this->rawBuffer.cnt;
//    this->rawBuffer.top = (this->rawBuffer.top + 1) % 6;
//  
//    do {
//        tempRaw.xAxis += this->rawBuffer.data[tempCnt].xAxis;
//        tempRaw.yAxis += this->rawBuffer.data[tempCnt].yAxis;
//        tempRaw.zAxis += this->rawBuffer.data[tempCnt].zAxis;
//        tempRaw.accelerator  += this->rawBuffer.data[tempCnt].accelerator;
//        ++tempCnt;
//        
//    } while (tempCnt < this->rawBuffer.cnt);
//
//    tempRaw.xAxis = tempRaw.xAxis / this->rawBuffer.cnt;
//    tempRaw.yAxis = tempRaw.yAxis / this->rawBuffer.cnt;
//    tempRaw.zAxis = tempRaw.zAxis / this->rawBuffer.cnt;
//    
//    tempRaw.accelerator = (tempRaw.xAxis * tempRaw.xAxis 
//            + tempRaw.yAxis * tempRaw.yAxis 
//            + tempRaw.zAxis * tempRaw.zAxis + tempRaw.accelerator) / this->rawBuffer.cnt;
//
//    
//    this->sensor.time = paraRaw->time;
//    this->sensor.accelerator = tempRaw.accelerator;
//}

void SmoothFilter::update(const RawData *paraRaw)
{
    uint32 tempCnt = 0;

    RawData tempRaw;

    if (this->rawBuffer.cnt > 0)
    {
        if ((paraRaw->time <= this->rawBuffer.back()->time) 
                || (paraRaw->time > (this->rawBuffer.back()->time + 100))) {
            this->rawBuffer.bottom = 0;
            this->rawBuffer.top = 0;
            this->rawBuffer.cnt = 0;
        }
    }
    
    if (this->rawBuffer.cnt > 5) {
        this->rawBuffer.bottom = (this->rawBuffer.bottom + 1) % 6;
        this->rawBuffer.cnt--;
    }
    
    this->rawBuffer.data[this->rawBuffer.top] = *paraRaw;
    
    ++this->rawBuffer.cnt;
    this->rawBuffer.top = (this->rawBuffer.top + 1) % 6;
  
    do {
        tempRaw.xAxis += this->rawBuffer.data[tempCnt].xAxis;
        tempRaw.yAxis += this->rawBuffer.data[tempCnt].yAxis;
        tempRaw.zAxis += this->rawBuffer.data[tempCnt].zAxis;
        tempRaw.accelerator  += this->rawBuffer.data[tempCnt].accelerator;
        ++tempCnt;
        
    } while (tempCnt < this->rawBuffer.cnt);

    tempRaw.xAxis = tempRaw.xAxis / this->rawBuffer.cnt;
    tempRaw.yAxis = tempRaw.yAxis / this->rawBuffer.cnt;
    tempRaw.zAxis = tempRaw.zAxis / this->rawBuffer.cnt;

    this->rawBuffer.back()->accelerator = (tempRaw.xAxis * tempRaw.xAxis 
        + tempRaw.yAxis * tempRaw.yAxis 
        + tempRaw.zAxis * tempRaw.zAxis);

    tempRaw.accelerator = (tempRaw.accelerator + this->rawBuffer.back()->accelerator) / this->rawBuffer.cnt;
	
    this->sensor.time = paraRaw->time;
    this->sensor.accelerator = tempRaw.accelerator;
}
